using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet.Dynamics
{
	/// <summary>
	/// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
	/// axis defines the orientation of the hinge axis
	/// </summary>
	public class HingeConstraint : TypedConstraint
	{
		private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints
		private JacobianEntry[] _jacAng = new JacobianEntry[2]; //2 orthogonal angular constraints

		private Vector3 _pivotInA;
		private Vector3 _pivotInB;
		private Vector3 _axisInA;
		private Vector3 _axisInB;

		private bool _angularOnly;

		public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)
			: base(rbA, rbB)
		{
			_pivotInA = pivotInA;
			_pivotInB = pivotInB;
			_axisInA = axisInA;
			_axisInB = -axisInB;
			_angularOnly = false;
		}

		public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)
			: base(rbA)
		{
			_pivotInA = pivotInA;
			_pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);
			_axisInA = axisInA;
			//fixed axis in worldspace
			_axisInB = Vector3.TransformNormal(-axisInA, rbA.CenterOfMassTransform);
			_angularOnly = false;
		}

		public HingeConstraint() { }

		public bool AngularOnly { set { _angularOnly = value; } }

		public override void BuildJacobian()
		{
			AppliedImpulse = 0f;

			Vector3 normal = new Vector3();

			if (!_angularOnly)
			{
				for (int i = 0; i < 3; i++)
				{
					MathHelper.SetElement(ref normal, i, 1);
					_jac[i] = new JacobianEntry(
							Matrix.Transpose(RigidBodyA.CenterOfMassTransform),
							Matrix.Transpose(RigidBodyB.CenterOfMassTransform),
							Vector3.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
							Vector3.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
							normal,
							RigidBodyA.InvInertiaDiagLocal,
							RigidBodyA.InverseMass,
							RigidBodyB.InvInertiaDiagLocal,
							RigidBodyB.InverseMass);
					MathHelper.SetElement(ref normal, i, 0);
				}
			}

			//calculate two perpendicular jointAxis, orthogonal to hingeAxis
			//these two jointAxis require equal angular velocities for both bodies
			//this is unused for now, it's a todo
			Vector3 axisWorldA = Vector3.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
			Vector3 jointAxis0 = new Vector3();
			Vector3 jointAxis1 = new Vector3();
			MathHelper.PlaneSpace1(axisWorldA, ref jointAxis0, ref jointAxis1);

			_jacAng[0] = new JacobianEntry(jointAxis0,
				Matrix.Transpose(RigidBodyA.CenterOfMassTransform),
				Matrix.Transpose(RigidBodyB.CenterOfMassTransform),
				RigidBodyA.InvInertiaDiagLocal,
				RigidBodyB.InvInertiaDiagLocal);

			_jacAng[1] = new JacobianEntry(jointAxis1,
				Matrix.Transpose(RigidBodyA.CenterOfMassTransform),
				Matrix.Transpose(RigidBodyB.CenterOfMassTransform),
				RigidBodyA.InvInertiaDiagLocal,
				RigidBodyB.InvInertiaDiagLocal);
		}

		public override void SolveConstraint(float timeStep)
		{
			Vector3 pivotAInW = Vector3.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
			Vector3 pivotBInW = Vector3.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);

			Vector3 normal = new Vector3(0, 0, 0);
			float tau = 0.3f;
			float damping = 1f;

			//linear part
			if (!_angularOnly)
			{
				for (int i = 0; i < 3; i++)
				{
					if (i == 0)
						normal = new Vector3(1, 0, 0);
					else if (i == 1)
						normal = new Vector3(0, 1, 0);
					else
						normal = new Vector3(0, 0, 1);

					float jacDiagABInv = 1f / _jac[i].Diagonal;

					Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
					Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

					Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
					Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
					Vector3 vel = vel1 - vel2;
					float rel_vel;
					rel_vel = Vector3.Dot(normal, vel);
					//positional error (zeroth order error)
					float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
					float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
					AppliedImpulse += impulse;
					Vector3 impulse_vector = normal * impulse;
					RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
					RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
				}
			}
			//solve angular part
			// get axes in world space
			Vector3 axisA = Vector3.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
			Vector3 axisB = Vector3.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);

			Vector3 angVelA = RigidBodyA.AngularVelocity;
			Vector3 angVelB = RigidBodyB.AngularVelocity;
			Vector3 angA = angVelA - axisA * Vector3.Dot(axisA, angVelA);
			Vector3 angB = angVelB - axisB * Vector3.Dot(axisB, angVelB);
			Vector3 velrel = angA - angB;

			//solve angular velocity correction
			float relaxation = 1f;
			float len = velrel.Length();
			if (len > 0.00001f)
			{
				Vector3 normal2 = Vector3.Normalize(velrel);
				float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
					RigidBodyB.ComputeAngularImpulseDenominator(normal2);
				// scale for mass and relaxation
				velrel *= (1f / denom) * 0.9f;
			}

			//solve angular positional correction
			Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
			float len2 = angularError.Length();
			if (len2 > 0.00001f)
			{
				Vector3 normal2 = Vector3.Normalize(angularError);
				float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
						RigidBodyB.ComputeAngularImpulseDenominator(normal2);
				angularError *= (1f / denom2) * relaxation;
			}

			RigidBodyA.ApplyTorqueImpulse(-velrel + angularError);
			RigidBodyB.ApplyTorqueImpulse(velrel - angularError);
		}

		public void UpdateRHS(float timeStep)
		{
		}
	}
}
